/*
 * Mostly stolen from http://willperone.net/Code/quaternion.php
 */
#ifndef _JPQUAT_H_
#define _JPQUAT_H_

#include <math.h>

#include "JPVec3.h"

class JPQuat
{
public:

	float x, y, z, s;

	//----------------------------------------
	// Constructors
	//----------------------------------------
	JPQuat():x(0.0f),y(0.0f),z(0.0f),s(0.0f) {}
	JPQuat(float px, float py, float pz, float r): x(px),y(py),z(pz),s(r) {}

	// from 3 euler angles
	JPQuat(float thetaX, float thetaY, float thetaZ)
	{
		float cos_z_2 = cosf(0.5f*thetaZ);
		float cos_y_2 = cosf(0.5f*thetaY);
		float cos_x_2 = cosf(0.5f*thetaX);

		float sin_z_2 = sinf(0.5f*thetaZ);
		float sin_y_2 = sinf(0.5f*thetaY);
		float sin_x_2 = sinf(0.5f*thetaX);

		// and now compute quaternion
		x = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
		y = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
		z = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;
		s = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
	}

	// from 3 euler angles 
	JPQuat(const JPVec3 &v)
	{	
		JPQuat(v.x,v.y,v.z);
	} 

	// from axis angle (radians)
	JPQuat(const JPVec3 &v,float a)
	{
		setAxisAngle(v,a);
	}

	//----------------------------------------
	// Public API
	//----------------------------------------

	void	setAxisAngle(const JPVec3 &axis, float angle);
	void	getAxisAngle(JPVec3 &axis, float &angle) const;
	void	conjugate();
	void	invert();
	void	normalise();

	JPQuat	getNormalised() const;
	JPVec3	rotateVec(const JPVec3 &v) const;

	float	length() const;
	float	sqrLength() const;

	//----------------------------------------
	// operator overloads
	//----------------------------------------
	// = operator
	JPQuat &operator =(const JPQuat &q)		
	{ 
		x = q.x; y = q.y; z = q.z; s = q.s;
		return *this; 
	}
	// + operator
	JPQuat operator +(const JPQuat &q) const { return JPQuat(x+q.x,y+q.y,z+q.z,s+q.s); }
	// - operator
	JPQuat operator -(const JPQuat &q) const { return JPQuat(x-q.x,y-q.y,z-q.z,s-q.s); }
	// * operator
	JPQuat operator *(const JPQuat &q) const	
	{	
		return JPQuat(	s*q.x + x*q.s + y*q.z - z*q.y,
						s*q.y - x*q.z + y*q.s + z*q.x,
						s*q.z + x*q.y - y*q.x + z*q.s,
						s*q.s - x*q.x - y*q.y - z*q.z );
	}
	// / operator
	JPQuat operator /(const JPQuat &q) const	
	{
		JPQuat p(q); 
		p.invert(); 
		return *this * p;
	}
	// * operator
	JPQuat operator *(float n) const	{ return JPQuat(x*n,y*n,z*n,s*n); }
	// / operator
	JPQuat operator /(float n) const	{ return JPQuat(x/n,y/n,z/n,s/n); }
	// - operator
	JPQuat operator -() const			{ return JPQuat(-x,-y,-z,-s); }
	// += operator
	JPQuat &operator +=(const JPQuat &q)
	{	
		x+=q.x; y+=q.y; z+=q.z; s+=q.s;
		return *this;
	}
	// -= operator
	JPQuat &operator -=(const JPQuat &q)
	{	
		x-=q.x; y-=q.y; z-=q.z; s-=q.s;
		return *this;
	}
	// *= operator
	JPQuat &operator *=(const JPQuat &q)		
	{	
		float nx = s*q.x + x*q.s + y*q.z - z*q.y;
		float ny = s*q.y - x*q.z + y*q.s + z*q.x;
		float nz = s*q.z + x*q.y - y*q.x + z*q.s;
		float ns = s*q.s - x*q.x - y*q.y - z*q.z;

		x = nx; y = ny; z = nz; s = ns;

		return *this;
	}
	// *= operator
	JPQuat &operator *= (float n)
	{ 
		x*=n; y*=n; z*=n; s*=n;
		return *this;
	}
	// /= operator
	JPQuat &operator /= (float n)
	{ 
		x/=n; y/=n; z/=n; s/=n;
		return *this;
	}
	// [] operator
	float operator [](int i)
	{
		// TO DO: assert if i < 0 || i >3
		if(i<0||i>3) i = 0;

		return *(&x+i);
	}
};

#endif	// _JPQUAT_H_